/*
 * camera.c
 *
 *  Created on: 2024年6月26日
 *      Author: Kenobi
 */
#include "camera.h"

uint8_t TFT18_User_data[LCDH][LCDW];  //存放裁剪原始图像数据
uint8_t TFT18_Bin_data[LCDH][LCDW];  //存放二值图像数据

// 常用基本变量
Lost_Line_Struct Lost;               // 丢线数结构体
Longest_White_Column_Struct LonWCol; // 最长白列结构体
Line_Edge_Struct Line;               // 边界结构体
Cross_Struct Cross;                  // 十字结构体
Barrier_Detect_Struct Barrier;       // 障碍物结构体
Zebra_Stripes_Struct Zebra_Stripes;  //斑马线结构体
battery_struct battery;              //电量结构体

// 加权控制
const uint8 Weight_L[LCDH] =
{
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端10 ——49 行权重
        1, 1, 1, 1, 1, 6, 7, 9, 11,13,         // 图像最远端20 ——59 行权重
        15,17,19,20,20,19,17,15,1, 1,         // 图像最远端40 ——19 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端30 ——09 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,         // 图像最远端50 ——29 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1        // 图像最远端00 ——39 行权重
};

const uint8 Weight_H[LCDH] =
    {
        1, 1, 1, 1, 1,1, 1, 6, 7, 9,         // 图像最远端20 ——59 行权重
        11,13,15,17,19,20,20,19,17,15,       // 图像最远端30 ——09 行权重
        1, 1, 1, 1, 1,1, 1, 1, 1, 1,        // 图像最远端40 ——19 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端50 ——29 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端00 ——39 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端10 ——49 行权重
};

const uint8 Weight_S[LCDH] =
    {
        1, 1, 1, 1, 1,1, 1, 6, 7, 9,         // 图像最远端20 ——59 行权重
        11,13,15,17,19,20,20,19,17,15,       // 图像最远端30 ——09 行权重
        1, 1, 1, 1, 1, 1, 1, 1, 1, 1,        // 图像最远端10 ——49 行权重
        11,13,15,17,19,20,20,19,17,15,       // 图像最远端30 ——09 行权重
        1, 1, 1, 1, 1,1, 1, 6, 7, 9,         // 图像最远端20 ——59 行权重
        1, 1, 1, 1, 1,1, 1, 1, 1, 1,        // 图像最远端40 ——19 行权重


};

const uint8_t Weight[LCDH]=                 //标准赛宽
{
        12,13,13,14,15,16,17,18,20,20,
        21,22,24,24,25,26,27,28,29,30,
        31,32,32,34,34,36,36,38,38,39,
        40,41,42,43,44,44,46,46,48,50,
        50,51,51,51,53,54,56,56,57,58,
        58,58,59,60,60,61,62,62,64,64
};

/*-------------------------------------------------------------------------------------------------------------------
  @brief     普通大津求阈值
  @param     image       图像数组
             col         列 ，宽度
             row         行，长度
  @return    threshold   返回int类型的的阈值
  Sample     threshold=my_adapt_threshold(mt9v03x_image[0],MT9V03X_W, MT9V03X_H);//普通大津
  @note      据说没有山威大津快，我感觉两个区别不大
-------------------------------------------------------------------------------------------------------------------*/
int My_Adapt_Threshold(uint8*image,uint16 width, uint16 height)   //大津算法，注意计算阈值的一定要是原图像
{
    #define GrayScale 256
    int pixelCount[GrayScale];
    float pixelPro[GrayScale];
    int i, j;
    int pixelSum = width * height/4;
    int  threshold = 0;
    uint8* data = image;  //指向像素数据的指针
    for (i = 0; i < GrayScale; i++)
    {
        pixelCount[i] = 0;
        pixelPro[i] = 0;
    }
    uint32 gray_sum=0;
    for (i = 0; i < height; i+=2)//统计灰度级中每个像素在整幅图像中的个数
    {
        for (j = 0; j <width; j+=2)
        {
            pixelCount[(int)data[i * width + j]]++;  //将当前的点的像素值作为计数数组的下标
            gray_sum+=(int)data[i * width + j];       //灰度值总和
        }
    }
    for (i = 0; i < GrayScale; i++) //计算每个像素值的点在整幅图像中的比例
    {
        pixelPro[i] = (float)pixelCount[i] / pixelSum;
    }
    float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;
    w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
    for (j = 0; j < GrayScale; j++)//遍历灰度级[0,255]
    {
        w0 += pixelPro[j];  //背景部分每个灰度值的像素点所占比例之和   即背景部分的比例
        u0tmp += j * pixelPro[j];  //背景部分 每个灰度值的点的比例 *灰度值
        w1=1-w0;
        u1tmp=gray_sum/pixelSum-u0tmp;
        u0 = u0tmp / w0;              //背景平均灰度
        u1 = u1tmp / w1;              //前景平均灰度
        u = u0tmp + u1tmp;            //全局平均灰度
        deltaTmp = w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);//平方
        if (deltaTmp > deltaMax)
        {
            deltaMax = deltaTmp;//最大类间方差法
            threshold = j;
        }
        if (deltaTmp < deltaMax)
        {
            break;
        }
    }
    if(threshold>255)
        threshold=255;
    if(threshold<0)
        threshold=0;
  return threshold;
}

//裁剪图像
void Get_Use_Image(uint8 mode)
{
    short i = 0, j = 0, row = 0, line = 0;
    if(mode==0)
    {
        for (i = 0; i < MT9V03X_H; i += 2)          //高 120 / 2  = 60
        {
            for (j = 0; j < MT9V03X_W; j += 2)     //宽188 / 2  = 94
            {
                TFT18_User_data[row][line] = mt9v03x_image[i][j];
                line++;
        }
        line = 0;
        row++;
        }
    }
//    else
//    {
//        for (i = 0; i < MT9V03X_H; i += 2)          //高 120 / 2  = 60
//        {
//            for (j = 0; j < MT9V03X_W; j += 2)     //宽188 / 2  = 94
//            {
//                TFT18_Bin_data[row][line] = Bin_data[i][j];
//                line++;
//            }
//        line = 0;
//        row++;
//        }
//    }
}


/*---------------------------------------------------------------
 【函    数】Bin_Image_Filter
 【功    能】过滤噪点
 【参    数】无
 【返 回 值】无
 【注意事项】
 ----------------------------------------------------------------*/
void Bin_Image_Filter (void)
{
    int16_t nr; //行
    int16_t nc; //列
    for (nr = 1; nr < LCDH - 1; ++nr)
    {
        for (nc = 1; nc < LCDW - 1; ++nc)
        {
            if ((TFT18_Bin_data[nr][nc] == 0)
                    && (TFT18_Bin_data[nr - 1][nc] + TFT18_Bin_data[nr + 1][nc] + TFT18_Bin_data[nr][nc + 1] + TFT18_Bin_data[nr][nc - 1] > 2*255))
            {
                TFT18_Bin_data[nr][nc] = 255;
            }
            else if ((TFT18_Bin_data[nr][nc] == 1)
                    && (TFT18_Bin_data[nr - 1][nc] + TFT18_Bin_data[nr + 1][nc] + TFT18_Bin_data[nr][nc + 1] + TFT18_Bin_data[nr][nc - 1] < 2*255))
            {
                TFT18_Bin_data[nr][nc] = 0;
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     图像二值化处理函数
  @param     原始图像数组，高度，宽度，二值化阈值
  @return    二值化之后的图像数组
  Sample     Image_Binarization(Threshold);//图像二值化
  @note      二值化处理，0x00黑，0xff是白，已经用宏定义替换过了
-------------------------------------------------------------------------------------------------------------------*/
void Image_Binarization(int threshold)//图像二值化
{
    uint16 i,j;
    for(i=0;i<LCDH;i++)
    {
        for(j=0;j<LCDW;j++)
        {
            if(TFT18_User_data[i][j]>=threshold)
                TFT18_Bin_data[i][j]=IMG_WHITE;//白
            else
                TFT18_Bin_data[i][j]=IMG_BLACK;//黑
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     双最长白列巡线
  @param     null
  @return    null
  Sample     Longest_White_Column_Left();
  @note      最长白列巡线，寻找初始边界，丢线，最长白列等基础元素，后续读取这些变量来进行赛道识别
-------------------------------------------------------------------------------------------------------------------*/
void Longest_White_Column() // 最长白列巡线
{
    int i, j;
    int left_border = 0, right_border = 0; // 临时存储赛道位置
    int start_column=10;//最长白列的搜索区间
    int end_column=LCDW-10;
    memset(&LonWCol, 0, sizeof(LonWCol)); // 初始化最长白列数据
    memset(&Lost, 0, sizeof(Lost));       // 初始化丢线数据

    //环岛需要对最长白列范围进行限定，环岛3状态找不到上角点，可以修改下述参数
    //环岛3状态需要改变最长白列寻找范围
    if(Island.Right_Island_Flag==1)//右环
    {
        if(Island.Island_State==3)
        {
            start_column=50;
            end_column=LCDW-10;
        }
    }
    else if(Island.Left_Island_Flag==1)//左环
    {
        if(Island.Island_State==3)
        {
            start_column=10;
            end_column=LCDW-30;
        }
    }

    // 从左到右，从下往上，遍历全图记录范围内的每一列白点数量
    for (j = start_column; j <= end_column; j++)
    {
        for (i = LCDH - 1; i >= 0; i--)
        {
            if (TFT18_Bin_data[i][j] == IMG_BLACK)
                break;
            else
                LonWCol.White_Column[j]++;
        }
    }

    // 从左到右找左边最长白列
    LonWCol.Left_Len_Col[0] = 0;
    for (i = start_column; i <= end_column; i++)
    {
        if (LonWCol.Left_Len_Col[0] < LonWCol.White_Column[i]) // 找最长的那一列
        {
            LonWCol.Left_Len_Col[0] = LonWCol.White_Column[i]; // 【0】是白列长度
            LonWCol.Left_Len_Col[1] = i;                       // 【1】是下标，第j列
        }
    }
    // 从右到左找右左边最长白列
    LonWCol.Right_Len_Col[0] = 0;
    for (i = start_column; i <= end_column; i++)
    {
        if (LonWCol.Right_Len_Col[0] < LonWCol.White_Column[i]) // 找最长的那一列
        {
            LonWCol.Right_Len_Col[0] = LonWCol.White_Column[i]; // 【0】是白列长度
            LonWCol.Right_Len_Col[1] = i;                       // 【1】是下标，第j列
        }
    }

    LonWCol.Search_Stop_Line = LonWCol.Left_Len_Col[0];           // 搜索截止行选取左或者右区别不大，他们两个理论上是一样的
    for (i = LCDH - 1; i >= LCDH - LonWCol.Search_Stop_Line; i--) // 常规巡线
    {
        for (j = LonWCol.Right_Len_Col[1]; j <= LCDW - 1 - 2; j++) // 右边往两边扫
        {
            if (TFT18_Bin_data[i][j] == IMG_WHITE && TFT18_Bin_data[i][j + 1] == IMG_BLACK && TFT18_Bin_data[i][j + 2] == IMG_BLACK) // 白黑黑，找到右边界
            {
                right_border = j;
                Lost.Right_Flag[i] = 0; // 右丢线数组，丢线置1，不丢线置0
                break;
            }
            else if (j >= LCDW - 1 - 2) // 没找到右边界，把屏幕最右赋值给右边界
            {
                right_border = j;
                Lost.Right_Flag[i] = 1; // 右丢线数组，丢线置1，不丢线置0
                break;
            }
        }
        for (j = LonWCol.Left_Len_Col[1]; j >= 0 + 2; j--) // 往左边扫描
        {
            if (TFT18_Bin_data[i][j] == IMG_WHITE && TFT18_Bin_data[i][j - 1] == IMG_BLACK && TFT18_Bin_data[i][j - 2] == IMG_BLACK) // 黑黑白认为到达左边界
            {
                left_border = j;
                Lost.Left_Flag[i] = 0; // 左丢线数组，丢线置1，不丢线置0
                break;
            }
            else if (j <= 0 + 2)
            {
                left_border = j;       // 找到头都没找到边，就把屏幕最左右当做边界
                Lost.Left_Flag[i] = 1; // 左丢线数组，丢线置1，不丢线置0
                break;
            }
        }
        Line.Left_Line[i] = left_border;   // 左边线线数组
        Line.Right_Line[i] = right_border; // 右边线线数组
        Line.Left_Line_Display[i] = Line.Left_Line[i];
        Line.Right_Line_Display[i] = Line.Right_Line[i];
    }

    for (i = LCDH - 1; i >= 0; i--) // 赛道数据初步分析
    {
        if (Lost.Left_Flag[i] == 1) // 单边丢线数
            Lost.Left_Num++;
        if (Lost.Right_Flag[i] == 1)
            Lost.Right_Num++;
        if (Lost.Left_Flag[i] == 1 && Lost.Right_Flag[i] == 1) // 双边丢线数
            Lost.All_Num++;
        if (Lost.Start_Left == 0 && Lost.Left_Flag[i] != 1) // 记录第一个非丢线点，边界起始点
            Lost.Start_Left = i;
        if (Lost.Start_Right == 0 && Lost.Right_Flag[i] != 1)
            Lost.Start_Right = i;
        Line.Road_Wide[i] = Line.Right_Line_Display[i] - Line.Left_Line_Display[i];
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左补线
  @param     补线的起点，终点
  @return    null
  Sample     Left_Add_Line(int x1,int y1,int x2,int y2);
  @note      补的直接是边界，点最好是可信度高的,不要乱补
-------------------------------------------------------------------------------------------------------------------*/
void Left_Add_Line(int x1, int y1, int x2, int y2)
{
    int i, max, a1, a2;
    int hx;
    if (x1 > LCDW - 1)
        x1 = LCDW - 1;
    else if (x1 <= 0)
        x1 = 0;
    if (y1 > LCDH - 1)
        y1 = LCDH - 1;
    else if (y1 <= 0)
        y1 = 0;
    if (x2 > LCDW - 1)
        x2 = LCDW - 1;
    else if (x2 <= 0)
        x2 = 0;
    if (y2 > LCDH - 1)
        y2 = LCDH - 1;
    else if (y2 <= 0)
        y2 = 0;
    a1 = y1;
    a2 = y2;
    if (a1 > a2)
    {
        max = a1;
        a1 = a2;
        a2 = max;
    }
    for (i = a1; i <= a2; i++)
    {
        hx = (i - y1) * (x2 - x1) / (y2 - y1) + x1;
        if (hx >= LCDW)
            hx = LCDW;
        else if (hx <= 0)
            hx = 0;
        Line.Left_Line_Display[i] = hx;
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右补线
  @param     补线的起点，终点
  @return    null
  Sample     Right_Add_Line(int x1,int y1,int x2,int y2);
  @note      补的直接是边界，点最好是可信度高的,不要乱补
-------------------------------------------------------------------------------------------------------------------*/
void Right_Add_Line(int x1, int y1, int x2, int y2)
{
    int i, max, a1, a2;
    int hx;
    if (x1 > LCDW - 1)
        x1 = LCDW - 1;
    else if (x1 <= 0)
        x1 = 0;
    if (y1 > LCDH - 1)
        y1 = LCDH - 1;
    else if (y1 <= 0)
        y1 = 0;
    if (x2 > LCDW - 1)
        x2 = LCDW - 1;
    else if (x2 <= 0)
        x2 = 0;
    if (y2 > LCDH - 1)
        y2 = LCDH - 1;
    else if (y2 <= 0)
        y2 = 0;
    a1 = y1;
    a2 = y2;
    if (a1 > a2)
    {
        max = a1;
        a1 = a2;
        a2 = max;
    }
    for (i = a1; i < a2; i++)
    {
        hx = (i - y1) * (x2 - x1) / (y2 - y1) + x1;
        if (hx >= LCDW)
            hx = LCDW;
        else if (hx <= 0)
            hx = 0;
        Line.Right_Line_Display[i] = hx;
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     找下面的两个拐点，供十字使用
  @param     搜索的范围起点，终点
  @return    修改两个全局变量
             Right_Down_Find=0;
             Left_Down_Find=0;
  Sample     Find_Down_Point(int start,int end)
  @note      运行完之后查看对应的变量，注意，没找到时对应变量将是0
-------------------------------------------------------------------------------------------------------------------*/
void Find_Down_Point(int start, int end)
{
    int i, t;
    Cross.Right_Down_Find = 0;
    Cross.Left_Down_Find = 0;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (start >= LCDH - 1 - 3) // 下面3行数据不稳定，不能作为边界点来判断，舍弃
        start = LCDH - 1 - 3;
    if (end <= LCDH - LonWCol.Search_Stop_Line)
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 3)
        end = 3;
    for (i = start; i >= end; i--)
    {
        if (Cross.Left_Down_Find == 0 &&                                            // 只找第一个符合条件的点
            abs(Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Left_Line_Display[i + 1] - Line.Left_Line_Display[i + 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Left_Line_Display[i + 2] - Line.Left_Line_Display[i + 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i - 2]) >= FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i - 3]) >= FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i - 4]) >= FOOT_THRESHOLD_LINE3_MIN)
        {
            Cross.Left_Down_Find = i; // 获取行数即可
        }
        if (Cross.Right_Down_Find == 0 &&                                             // 只找第一个符合条件的点
            abs(Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Right_Line_Display[i + 1] - Line.Right_Line_Display[i + 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Right_Line_Display[i + 2] - Line.Right_Line_Display[i + 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i - 2]) <= -FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i - 3]) <= -FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i - 4]) <= -FOOT_THRESHOLD_LINE3_MIN)
        {
            Cross.Right_Down_Find = i;
        }
        if (Cross.Left_Down_Find != 0 && Cross.Right_Down_Find != 0) // 两个找到就退出
        {
            break;
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     找上面的两个拐点，供十字使用
  @param     搜索的范围起点，终点
  @return    修改两个全局变量
             Left_Up_Find=0;
             Right_Up_Find=0;
  Sample     Find_Up_Point(int start,int end)
  @note      运行完之后查看对应的变量，注意，没找到时对应变量将是0
-------------------------------------------------------------------------------------------------------------------*/
void Find_Up_Point(int start, int end)
{
    int i, t;
    Cross.Left_Up_Find = 0;
    Cross.Right_Up_Find = 0;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (end <= LCDH - LonWCol.Search_Stop_Line)
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 3) // 即使最长白列非常长，也要舍弃部分点，防止数组越界
        end = 3;
    if (start >= LCDH - 1 - 3) // 下面3行数据不稳定，不能作为边界点来判断，舍弃
        start = LCDH - 1 - 3;

    for (i = start; i >= end; i--)
    {
        if (Cross.Left_Up_Find == 0 &&                                              // 只找第一个符合条件的点
            abs(Line.Left_Line_Display[i] - Line.Left_Line_Display[i - 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Left_Line_Display[i - 1] - Line.Left_Line_Display[i - 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Left_Line_Display[i - 2] - Line.Left_Line_Display[i - 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 2]) >= FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 3]) >= FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 4]) >= FOOT_THRESHOLD_LINE3_MIN)
        {
            Cross.Left_Up_Find = i; // 获取行数即可
        }
        if (Cross.Right_Up_Find == 0 &&                                               // 只找第一个符合条件的点
            abs(Line.Right_Line_Display[i] - Line.Right_Line_Display[i - 1]) <= FOOT_THRESHOLD_MAX && // 角点的阈值可以更改
            abs(Line.Right_Line_Display[i - 1] - Line.Right_Line_Display[i - 2]) <= FOOT_THRESHOLD_MAX &&
            abs(Line.Right_Line_Display[i - 2] - Line.Right_Line_Display[i - 3]) <= FOOT_THRESHOLD_MAX &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 2]) <= -FOOT_THRESHOLD_LINE1_MIN &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 3]) <= -FOOT_THRESHOLD_LINE2_MIN &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 4]) <= -FOOT_THRESHOLD_LINE3_MIN)
        {
            Cross.Right_Up_Find = i;
        }
        if (Cross.Left_Up_Find != 0 && Cross.Right_Up_Find != 0) // 两个找到就退出
        {
            break;
        }
    }
    if (abs(Cross.Right_Up_Find - Cross.Left_Up_Find) >= TREA_MAX) // 纵向撕裂过大，视为误判
    {
        Cross.Right_Up_Find = 0;
        Cross.Left_Up_Find = 0;
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左边界延长
  @param     延长起始行数，延长到某行
  @return    null
  Sample     Stop_Detect(void)
  @note      从起始点向上找5个点，算出斜率，向下延长，直至结束点
-------------------------------------------------------------------------------------------------------------------*/
void Lengthen_Left_Boundry(int start, int end)
{
    int i, t;
    float k = 0;
    if (start >= LCDH - 1) // 起始点位置校正，排除数组越界的可能
        start = LCDH - 1;
    else if (start <= 0)
        start = 0;
    if (end >= LCDH - 1)
        end = LCDH - 1;
    else if (end <= 0)
        end = 0;
    if (end < start) //++访问，坐标互换
    {
        t = end;
        end = start;
        start = t;
    }

    if (start <= 3) // 因为需要在开始点向上找3个点，对于起始点过于靠上，不能做延长，只能直接连线
    {
        Left_Add_Line(Line.Left_Line_Display[start], start, Line.Left_Line_Display[end], end);
    }

    else
    {
        k = (float)(Line.Left_Line_Display[start] - Line.Left_Line_Display[start - 4]) / 5.0; // 这里的k是1/斜率
        for (i = start; i <= end; i++)
        {
            Line.Left_Line_Display[i] = (int)(i - start) * k + Line.Left_Line_Display[start]; //(x=(y-y1)*k+x1),点斜式变形
            if (Line.Left_Line_Display[i] >= LCDW - 1)
            {
                Line.Left_Line_Display[i] = LCDW - 1;
            }
            else if (Line.Left_Line_Display[i] <= 0)
            {
                Line.Left_Line_Display[i] = 0;
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右左边界延长
  @param     延长起始行数，延长到某行
  @return    null
  Sample     Stop_Detect(void)
  @note      从起始点向上找3个点，算出斜率，向下延长，直至结束点
-------------------------------------------------------------------------------------------------------------------*/
void Lengthen_Right_Boundry(int start, int end)
{
    int i, t;
    float k = 0;
    if (start >= LCDH - 1) // 起始点位置校正，排除数组越界的可能
        start = LCDH - 1;
    else if (start <= 0)
        start = 0;
    if (end >= LCDH - 1)
        end = LCDH - 1;
    else if (end <= 0)
        end = 0;
    if (end < start) //++访问，坐标互换
    {
        t = end;
        end = start;
        start = t;
    }

    if (start <= 5) // 因为需要在开始点向上找3个点，对于起始点过于靠上，不能做延长，只能直接连线
    {
        Right_Add_Line(Line.Right_Line_Display[start], start, Line.Right_Line_Display[end], end);
    }
    else
    {
        k = (float)(Line.Right_Line_Display[start] - Line.Right_Line_Display[start - 4]) / 5.0; // 这里的k是1/斜率
        for (i = start; i <= end; i++)
        {
            Line.Right_Line_Display[i] = (int)(i - start) * k + Line.Right_Line_Display[start]; //(x=(y-y1)*k+x1),点斜式变形
            if (Line.Right_Line_Display[i] >= LCDW - 1)
            {
                Line.Right_Line_Display[i] = LCDW - 1;
            }
            else if (Line.Right_Line_Display[i] <= 0)
            {
                Line.Right_Line_Display[i] = 0;
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     十字检测
  @param     null
  @return    null
  Sample     Cross_Detect(void);
  @note      利用四个拐点判别函数，查找四个角点，根据找到拐点的个数决定是否补线
-------------------------------------------------------------------------------------------------------------------*/
void Cross_Detect()
{
    int down_search_start = 0; // 下点搜索开始行

    Cross.Cross_Flag = 0;
    Cross.Left_Up_Find = 0;
    Cross.Right_Up_Find = 0;
    if (Lost.All_Num >= 10) // 十字必定有双边丢线，在有双边丢线的情况下再开始找角点
    {
        Find_Up_Point(LCDH - 1, 0);
        if (Cross.Left_Up_Find == 0 && Cross.Right_Up_Find == 0) // 只要没有同时找到两个上点，直接结束
        {
            return;
        }
    }
    if (Cross.Left_Up_Find != 0 && Cross.Right_Up_Find != 0) // 找到两个上点，就找到十字了
    {
        Cross.Cross_Flag = 1;                                                                                    // 对应标志位，便于各元素互斥掉
        down_search_start = Cross.Left_Up_Find > Cross.Right_Up_Find ? Cross.Left_Up_Find : Cross.Right_Up_Find; // 用两个上拐点坐标靠下者作为下点的搜索上限
        Find_Down_Point(LCDH - 5, down_search_start + 2);                                                        // 在上拐点下2行作为下点的截止行
        if (Cross.Left_Down_Find <= Cross.Left_Up_Find)
        {
            Cross.Left_Down_Find = 0; // 下点不可能比上点还靠上
        }
        if (Cross.Right_Down_Find <= Cross.Right_Up_Find)
        {
            Cross.Right_Down_Find = 0; // 下点不可能比上点还靠上
        }
        if (Cross.Left_Down_Find != 0 && Cross.Right_Down_Find != 0)
        { // 四个点都在
            Left_Add_Line(Line.Left_Line_Display[Cross.Left_Up_Find], Cross.Left_Up_Find, Line.Left_Line_Display[Cross.Left_Down_Find], Cross.Left_Down_Find);
            Right_Add_Line(Line.Right_Line_Display[Cross.Right_Up_Find], Cross.Right_Up_Find, Line.Right_Line_Display[Cross.Right_Down_Find], Cross.Right_Down_Find);
        }
        else if (Cross.Left_Down_Find == 0 && Cross.Right_Down_Find != 0) // 11//这里使用的都是斜率补线
        {                                                                 // 三个点                                     //01
            Lengthen_Left_Boundry(Cross.Left_Up_Find - 1, LCDH - 1);
            Right_Add_Line(Line.Right_Line_Display[Cross.Right_Up_Find], Cross.Right_Up_Find, Line.Right_Line_Display[Cross.Right_Down_Find], Cross.Right_Down_Find);
        }
        else if (Cross.Left_Down_Find != 0 && Cross.Right_Down_Find == 0) // 11
        {                                                                 // 三个点                                     //10
            Left_Add_Line(Line.Left_Line_Display[Cross.Left_Up_Find], Cross.Left_Up_Find, Line.Left_Line_Display[Cross.Left_Down_Find], Cross.Left_Down_Find);
            Lengthen_Right_Boundry(Cross.Right_Up_Find - 1, LCDH - 1);
        }
        else if (Cross.Left_Down_Find == 0 && Cross.Right_Down_Find == 0) // 11
        {                                                                 // 就俩上点                                   //00
            Lengthen_Left_Boundry(Cross.Left_Up_Find - 1, LCDH - 1);
            Lengthen_Right_Boundry(Cross.Right_Up_Find - 1, LCDH - 1);
        }
    }
    //    tft180_show_string(0, 13*7, "U_Left:");tft180_show_int(50, 13*7, Cross.Left_Down_Find, 2);
    //    tft180_show_string(0, 13*8, "U_Right:");tft180_show_int(50, 13*8, Cross.Right_Up_Find, 2);
    //    tft180_show_string(0, 13*9, "D_Left:");tft180_show_int(50, 13*9, Cross.Left_Down_Find, 2);
    //    tft180_show_string(0, 13*10, "D_Right:");tft180_show_int(50, 13*10, Cross.Right_Down_Find, 2);
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     直道检测
  @param     null
  @return    null
  Sample     Straight_Detect()；
  @note      利用最长白列，边界起始点，中线起始点，
-------------------------------------------------------------------------------------------------------------------*/
void Straight_Detect(void)
{
    LonWCol.Straight_Flag=0;
    if(Island.Island_State)return;
    if(LonWCol.Search_Stop_Line>=35)//截止行很远
    {
        if(Lost.Start_Left>=50&&Lost.Start_Right>=50)//起始点靠下
        {
            if(-10<=Err&&Err<=10)//误差很小
            {
                LonWCol.Straight_Flag=1;//认为是直道
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     找上面的两个拐点，供避障使用
  @param     搜索的范围起点，终点
  @return    修改两个全局变量
             Left_Find=0;
             Right_Find=0;
  Sample     Find_Up_Point_For_Barrier(int start,int end)
  @note      运行完之后查看对应的变量，注意，没找到时对应变量将是0
-------------------------------------------------------------------------------------------------------------------*/
void Find_Up_Point_For_Barrier(int start, int end)
{
    int i, t;
    Barrier.Left_Find = 0;
    Barrier.Right_Find = 0;
    if (start < end)
    {
        t = start;
        start = end;
        end = t;
    }
    if (end <= LCDH - LonWCol.Search_Stop_Line)
        end = LCDH - LonWCol.Search_Stop_Line;
    if (end <= 3) // 即使最长白列非常长，也要舍弃部分点，防止数组越界
        end = 3;
    if (start >= LCDH - 1 - 3) // 下面3行数据不稳定，不能作为边界点来判断，舍弃
        start = LCDH - 1 - 3;

    for (i = start; i >= end; i--)
    {
        if (Barrier.Left_Find == 0 &&                                              // 只找第一个符合条件的点
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 2]) >= 10 &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 3]) >= 15 &&
            (Line.Left_Line_Display[i] - Line.Left_Line_Display[i + 4]) >= 15)
        {
            Barrier.Left_Find = i; // 获取行数即可
        }
        if (Barrier.Right_Find == 0 &&                                               // 只找第一个符合条件的点
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 2]) <= -10 &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 3]) <= -15 &&
            (Line.Right_Line_Display[i] - Line.Right_Line_Display[i + 4]) <= -15)
        {
            Barrier.Right_Find = i;
        }
        if (Barrier.Left_Find != 0 && Barrier.Right_Find != 0) // 两个找到就退出
        {
            return;
        }
    }
}

///*-------------------------------------------------------------------------------------------------------------------
//  @brief     障碍物检测
//  @param     null
//  @return    null
//  Sample     Barrier_Detect()；
//  @note
//-------------------------------------------------------------------------------------------------------------------*/
int sum=0;
void Barrier_Detect(void)
{
    Barrier.Barrier_Flag=0;
    if(timer_Zebra==0)return;
    int i;sum=0;
    if(Island.Island_State || Cross.Cross_Flag || Zebra_Stripes.Zebra_Stripes_Flag || Car_Status==STOP)
        return;

    for(i = LCDH - 5; i >= LCDH - LonWCol.Search_Stop_Line; i--)
    {
        if((Weight[i] - (Line.Right_Line_Display[i]-Line.Left_Line_Display[i]))>2 &&
           (Weight[i+1] - (Line.Right_Line_Display[i+1]-Line.Left_Line_Display[i+1])>3) &&
           (Weight[i+2] - (Line.Right_Line_Display[i+2]-Line.Left_Line_Display[i+2])>4) &&
           (Weight[i+3] - (Line.Right_Line_Display[i+3]-Line.Left_Line_Display[i+3])>4) &&
           Lost.Left_Flag[i]==0 && Lost.Right_Flag[i]==0 &&
           Lost.Left_Flag[i+1]==0 && Lost.Right_Flag[i+1]==0 &&
           Lost.Left_Flag[i+2]==0 && Lost.Right_Flag[i+2]==0 &&
           Lost.Left_Flag[i+3]==0 && Lost.Right_Flag[i+3]==0)
        {
            ++sum;
        }
    }
    if(sum>=3)
    {
        Barrier.Barrier_Flag=1;
    }
    else {
        Barrier.Barrier_Flag=0;
    }

    if(Barrier.Barrier_Flag)
    {

        gpio_set_level(LED1,1);

        if(key_get_state(P2))
        {
            Barrier.Barrier_Left_Flag=1;
            Barrier.Barrier_Right_Flag=0;
        }
        else
        {
            Barrier.Barrier_Right_Flag=1;
            Barrier.Barrier_Left_Flag=0;
        }
        if(Barrier.Barrier_Left_Flag)
        {
            for (i = 1; i < LCDH-1; ++i)
            {
                Line.Left_Line_Display[i]=(LCDW-Weight[i])/2+25;
                Line.Right_Line_Display[i]=LCDW-(LCDW-Weight[i])/2+25;
                Line.Left_Line_Display[i] = func_limit_ab(Line.Left_Line_Display[i], 1, LCDW-1);
                Line.Right_Line_Display[i] = func_limit_ab(Line.Right_Line_Display[i], 1, LCDW-1);

//                  Line.Left_Line_Display[i]+=20;
//                  Line.Right_Line_Display[i]+=20;
            }
        }
        if(Barrier.Barrier_Right_Flag)
        {
            for (i = 1; i < LCDH-1; ++i)
            {
                Line.Right_Line_Display[i]=LCDW-(LCDW-Weight[i])/2-25;
                Line.Left_Line_Display[i]=(LCDW-Weight[i])/2-25;
                Line.Left_Line_Display[i] = func_limit_ab(Line.Left_Line_Display[i], 1, LCDW-1);
                Line.Right_Line_Display[i] = func_limit_ab(Line.Right_Line_Display[i], 1, LCDW-1);

//                  Line.Left_Line_Display[i]-=20;
//                  Line.Right_Line_Display[i]-=20;
            }

        }


//        Longest_White_Column();                        // 刷新边界数据
    }
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     斑马线检测
  @param     null
  @return    null
  Sample     Zebra_Stripes_Detect(void)
  @note      边界起始靠下，最长白列较长，赛道宽度过窄，且附近大量跳变
-------------------------------------------------------------------------------------------------------------------*/
void Zebra_Stripes_Detect(void)
{
    Zebra_Stripes.Zebra_Stripes_Flag=0;
    if(timer_Zebra==0)return;
    Zebra_Stripes.change_count=0;//跳变计数
    int start_line=LCDH-10;
    int endl_ine=5;
    if(Cross.Cross_Flag /*|| Island.Island_State*/)//元素互斥，不是十字,不是环岛
        return;
    if(LonWCol.Search_Stop_Line>=35)
        for(int i=start_line;i>=endl_ine;i--)//区域内跳变计数
        {
            if(LonWCol.White_Column[i]-LonWCol.White_Column[i+1]>=10)
            {
                Zebra_Stripes.change_count++;
            }
        }
    if(Zebra_Stripes.change_count>=3)//跳变大于某一阈值，认为找到了斑马线
    {
        Zebra_Stripes.Zebra_Stripes_Flag=1;
    }
//    tft180_show_int(0, 8*10, Zebra_Stripes.change_count, 2);
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     摄像头误差获取
  @param     null
  @return    获取到的误差
  Sample     err=Err_Sum();
  @note      加权取平均
-------------------------------------------------------------------------------------------------------------------*/
float Err_Sum(void)
{
    int i;
    float err = 0;
    float weight_count = 0;
    // 常规误差
    for (i = LCDH - 1; i >= LCDH - LonWCol.Search_Stop_Line - 1; i--) // 常规误差计算
    {
        if(Island.Island_State>0&&Island.Island_State<8)
        {
            err += (LCDW / 2 - ((Line.Left_Line_Display[i] + Line.Right_Line_Display[i]) >> 1)) * Weight_L[i]; // 右移1位，等效除2
            weight_count += Weight_L[i];
        }
        else
        {
            err += (LCDW / 2 - ((Line.Left_Line_Display[i] + Line.Right_Line_Display[i]) >> 1)) * Weight_H[i]; // 右移1位，等效除2
            weight_count += Weight_H[i];
        }
    }
    err = err / weight_count;

    return err;
}

float Err_Sum_side(void)
{
    int i;
    float err = 0;
    float weight_count = 0;
    // 常规误差
    for (i = LCDH - 1; i >= LCDH - LonWCol.Search_Stop_Line - 1; i--) // 常规误差计算
    {
        err += (LCDW / 2 - ((Line.Left_Line_Display[i] + Line.Right_Line_Display[i]) >> 1)) * Weight_S[i]; // 右移1位，等效除2
        weight_count += Weight_S[i];
    }
    err = err / weight_count;

    return err;
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     绘制边界
  @param     null
  @return    null
-------------------------------------------------------------------------------------------------------------------*/
void BinRoadSide()
{
    MyRoadGetMidPosition();
    zf_assert(LonWCol.Search_Stop_Line<=LCDH);
    for (uint8_t i = LCDH - 1; i > LCDH - LonWCol.Search_Stop_Line; --i)
    {
        func_limit_ab(Line.Left_Line_Display[i], 1, LCDW);
        func_limit_ab(Line.Mid_Line[i], 1, LCDW);
        func_limit_ab(Line.Right_Line_Display[i], 1, LCDW);

        // 绘制左侧车道的点
        tft180_draw_point(Line.Left_Line_Display[i], i, RGB565_RED);
        // 绘制中线的点
        tft180_draw_point(Line.Mid_Line[i], i, RGB565_RED);
        // 绘制右侧车道的点
        tft180_draw_point(Line.Right_Line_Display[i], i, RGB565_RED);
    }
}

float my_quxian(uint8_t *data, int start, int end)
{
    uint8_t j=0;
    float ret=0;
    for (uint8_t i = start+1; i < end-1; i++)
    {
        ret += (i - j) * (data[j+4] - data[i]) - (data[i] - data[j]) * (j+4 - i);
        //fabs((b.x - a.x) * (c.y - b.y) - (b.y - a.y) * (c.x - b.x)) < epsilon;
        j++;
        //printf("%.4f\n", a);
    }
    return ret;
}

// 求方差
int variance(uint8_t *data, int start, int end)
{
    int sum = 0;
    int mean = 0;
    int variance = 0;
    int i;
    for (i = start; i < end; i++)
    {
        sum += (data[i]);
    }
    mean = sum / (end-start);
    for (i = start; i < end; i++)
    {
        variance += (data[i] - mean) * (data[i] - mean);
    }
    variance/=(end-start);

    return variance;
}

// 绘制中线
void MyRoadGetMidPosition()
{
    for (int i = 0; i < LCDH; i++)
    {
        Line.Mid_Line[i] = (Line.Left_Line_Display[i] + Line.Right_Line_Display[i]) >> 1;
    }
}

void battery_voltage_init()
{
    adc_init(ADC_CHANNEL1, ADC_12BIT);                                          // 初始化对应 ADC 通道为对应精度

}

uint8 battery_voltage_get()
{
    battery.adc_reg_value = adc_convert(ADC_CHANNEL1);
       // 这里的5.7是分压电阻的倍数
   battery.voltage = (float)battery.adc_reg_value / 4095  * 3300 * 5.7;
   if(11700 > battery.voltage)
   {
       if(battery._11_7_num++ < 1000)
           battery._11_7v_Flag=0;
       else
           battery._11_7v_Flag=1;
   }
   else
       battery._11_7_num=0;

   if(11000 > battery.voltage)
     {
         if(battery._11_0_num++ < 1000)
             battery._11_0v_Flag=0;
         else
             battery._11_0v_Flag=1;
     }
     else
         battery._11_0_num=0;

       if((BLDC_MIN_VOLTAGE > battery.voltage))
       {
           // 5000次都小于阈值，则说明电压过低
           if(battery.low_power_num++ < 5000)
           {
               return 1;
           }
       }
       else
       {
           battery.low_power_num = 0;
       }
       return 0;
}
